/*=================================================================
 * adjustment.cpp does adjustment computation
 *
 * Dr.Kanok Weerawong
 * 29/04/2010
 *=================================================================*/
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdlib.h>
#include <math.h>
using namespace std;
#include "matrixManipulation.h"

struct TimeStruct {
	double year;
	double month;
	double day;
	double hour;
	double minute;
	double second;
};
struct DimapDataStruct {
	int nRows;
	TimeStruct refTime;
	int refLine;
	double linePeriod;
	double xyLos[8];
	double yawPitchRoll[3];
	double uv[2];
	int nXYZs;
	TimeStruct *tXYZ;
	double *XArray;
	double *YArray;
	double *ZArray;
	int nQuaternions;
	TimeStruct *tQuaternion;
	double *q0Array;
	double *q1Array;
	double *q2Array;
	double *q3Array;
};
struct ImagePointStruct {
	string id;
	bool flag;
	double x;
	double sigmax;
	double y;
	double sigmay;
};
struct GroundPointStruct {
	string id;
	char pointType;
	double X;
	double Y;
	double Z;
	double sigmaX;
	double sigmaY;
	double sigmaZ;
	struct ImagePointStruct *imagePoints;
};

#define N_EQN 2
#define N_PAR 18
#define N_XYZ 3

void adjustment(double *centralRow, TimeStruct *refTime, double *refLine, double *linePeriod,
				double **xyLos, int nPoints, int mImages, int totalImagePoints,
				double **imagePar0, double **Delta0, double **sigmaImagePar0,
				int nAdjustedPar, GroundPointStruct *groundPoints, ofstream &outputfp)
{
	int nIterations = 0;
	bool isContinued = true;
	bool isOverThreshold = false;

	double sigma0hat2;
	double ux, uy, uz, XL, YL, ZL, omega, phi, kappa, Xi, Yi, Zi;
	double dm11dOmega, dm12dOmega, dm13dOmega, dm21dOmega, dm22dOmega, dm23dOmega, dm31dOmega, dm32dOmega, dm33dOmega;
	double dm11dPhi, dm12dPhi, dm13dPhi, dm21dPhi, dm22dPhi, dm23dPhi, dm31dPhi, dm32dPhi, dm33dPhi;
	double dm11dKappa, dm12dKappa, dm13dKappa, dm21dKappa, dm22dKappa, dm23dKappa, dm31dKappa, dm32dKappa, dm33dKappa;
	double dUdOmega, dVdOmega, dWdOmega, dUdPhi, dVdPhi, dWdPhi, dUdKappa, dVdKappa, dWdKappa;
	double dUdXL, dVdXL, dWdXL, dUdYL, dVdYL, dWdYL, dUdZL, dVdZL, dWdZL;
	double dUdXi, dVdXi, dWdXi, dUdYi, dVdYi, dWdYi, dUdZi, dVdZi, dWdZi;

	// Memory allocation for matrices and vector
	double **mOmega    = new double*[3];
	double **mPhi      = new double*[3];
	double **mKappa    = new double*[3];
	double **xyzVector = new double*[3];
	for (int i=0 ; i < 3 ; i++){
		mOmega[i]    = new double[3];
		mPhi[i]      = new double[3];
		mKappa[i]    = new double[3];
		xyzVector[i] = new double[1];
	}
	
	double **B   = new double*[N_EQN*totalImagePoints];
	double **F   = new double*[N_EQN*totalImagePoints];
	double **W   = new double*[N_EQN*totalImagePoints];
	double **v   = new double*[N_EQN*totalImagePoints];
	for (int i=0 ; i < N_EQN*totalImagePoints ; i++){
		B[i] = new double[N_PAR*mImages+N_XYZ*nPoints];
		F[i] = new double[1];
		W[i] = new double[N_EQN*totalImagePoints];
		v[i] = new double[1];
	}
	double **Wdot = new double*[N_PAR*mImages+N_XYZ*nPoints];
	double **Fdot = new double*[N_PAR*mImages+N_XYZ*nPoints];
	double **vdot  = new double*[N_PAR*mImages+N_XYZ*nPoints];
	for (int i=0 ; i < N_PAR*mImages+N_XYZ*nPoints ; i++){
		Wdot[i] = new double[N_PAR*mImages+N_XYZ*nPoints];
		Fdot[i] = new double[1];
		vdot[i]  = new double[1];
	}
	matrixSetZero(Fdot,N_PAR*mImages+N_XYZ*nPoints,1);

	// Compute redundancy
	int adjustedPoints = 0;
	for (int j=0 ; j<nPoints ; j++){
		if (groundPoints[j].pointType == 'T'){
			adjustedPoints++;
		}
	}
	int redundancy = N_EQN*totalImagePoints - adjustedPoints*N_XYZ - nAdjustedPar;

	// Start adjustment loop
	do {
		nIterations++;
		int imagePointIndex = 0;
		matrixSetZero(B,N_EQN*totalImagePoints,N_PAR*mImages+N_XYZ*nPoints);
		matrixSetZero(W,N_EQN*totalImagePoints,N_EQN*totalImagePoints);
		matrixSetZero(Wdot,N_PAR*mImages+N_XYZ*nPoints,N_PAR*mImages+N_XYZ*nPoints);

		// Looping through all the points
		for (int j=0 ; j<nPoints ; j++){
			Xi = groundPoints[j].X;
			Yi = groundPoints[j].Y;
			Zi = groundPoints[j].Z;

			// Looping through all the images of this point
			for (int i=0 ; i<mImages ; i++){
				// Check if this point wasn't measured on this image, skip.
				if (groundPoints[j].imagePoints[i].flag){
					// Compute values of imaging parameters using polynomials
					double lines   = groundPoints[j].imagePoints[i].y;
					double samples = groundPoints[j].imagePoints[i].x;
					// t=0 at center
					double t = linePeriod[i]*(lines-centralRow[i]);// seconds
					XL    = imagePar0[i][0] + Delta0[i][0]  + Delta0[i][1]*t  + Delta0[i][2]*t*t;
					YL    = imagePar0[i][1] + Delta0[i][3]  + Delta0[i][4]*t  + Delta0[i][5]*t*t;
					ZL    = imagePar0[i][2] + Delta0[i][6]  + Delta0[i][7]*t  + Delta0[i][8]*t*t;
					omega = imagePar0[i][3] + Delta0[i][9]  + Delta0[i][10]*t + Delta0[i][11]*t*t;
					phi   = imagePar0[i][4] + Delta0[i][12] + Delta0[i][13]*t + Delta0[i][14]*t*t;
					kappa = imagePar0[i][5] + Delta0[i][15] + Delta0[i][16]*t + Delta0[i][17]*t*t;

					// Set up basic rotation matrices
					mOmega[0][0] = 1;
					mOmega[0][1] = 0;
					mOmega[0][2] = 0;
					mOmega[1][0] = 0;
					mOmega[1][1] = cos(omega);
					mOmega[1][2] = sin(omega);
					mOmega[2][0] = 0;
					mOmega[2][1] =-sin(omega);
					mOmega[2][2] = cos(omega);

					mPhi[0][0] = cos(phi);
					mPhi[0][1] = 0;
					mPhi[0][2] =-sin(phi);
					mPhi[1][0] = 0;
					mPhi[1][1] = 1;
					mPhi[1][2] = 0;
					mPhi[2][0] = sin(phi);
					mPhi[2][1] = 0;
					mPhi[2][2] = cos(phi);

					mKappa[0][0] = cos(kappa);
					mKappa[0][1] = sin(kappa);
					mKappa[0][2] = 0;
					mKappa[1][0] =-sin(kappa);
					mKappa[1][1] = cos(kappa);
					mKappa[1][2] = 0;
					mKappa[2][0] = 0;
					mKappa[2][1] = 0;
					mKappa[2][2] = 1;

					double **mOpk = matrixMultiplyABC(mKappa, mPhi,mOmega, 3, 3, 3, 3);

					dm11dOmega = 0;
					dm12dOmega =-sin(omega)*sin(kappa)+cos(omega)*sin(phi)*cos(kappa);
					dm13dOmega = cos(omega)*sin(kappa)+sin(omega)*sin(phi)*cos(kappa);
					dm21dOmega = 0;
					dm22dOmega =-sin(omega)*cos(kappa)-cos(omega)*sin(phi)*sin(kappa);
					dm23dOmega = cos(omega)*cos(kappa)-sin(omega)*sin(phi)*sin(kappa);
					dm31dOmega = 0;
					dm32dOmega =-cos(omega)*cos(phi);
					dm33dOmega =-sin(omega)*cos(phi);

					dm11dPhi =-sin(phi)*cos(kappa);
					dm12dPhi = sin(omega)*cos(phi)*cos(kappa);
					dm13dPhi =-cos(omega)*cos(phi)*cos(kappa);
					dm21dPhi = sin(phi)*sin(kappa);
					dm22dPhi =-sin(omega)*cos(phi)*sin(kappa);
					dm23dPhi = cos(omega)*cos(phi)*sin(kappa);
					dm31dPhi = cos(phi);
					dm32dPhi = sin(omega)*sin(phi);
					dm33dPhi =-cos(omega)*sin(phi);

					dm11dKappa =-cos(phi)*sin(kappa);
					dm12dKappa = cos(omega)*cos(kappa)-sin(omega)*sin(phi)*sin(kappa);
					dm13dKappa = sin(omega)*cos(kappa)+cos(omega)*sin(phi)*sin(kappa);
					dm21dKappa =-cos(phi)*cos(kappa);
					dm22dKappa =-cos(omega)*sin(kappa)-sin(omega)*sin(phi)*cos(kappa);
					dm23dKappa =-sin(omega)*sin(kappa)+cos(omega)*sin(phi)*cos(kappa);
					dm31dKappa = 0;
					dm32dKappa = 0;
					dm33dKappa = 0;

					// Compute ux, uy, uz, U, V, W
				    double psiX = (xyLos[i][0] + samples*xyLos[i][1] + samples*samples*xyLos[i][2] 
								+ samples*samples*samples*xyLos[i][3])/1000.0;
					double psiY = (xyLos[i][4] + samples*xyLos[i][5] + samples*samples*xyLos[i][6] 
								+ samples*samples*samples*xyLos[i][7])/1000.0;

					ux =  tan(psiY);
					uy = -tan(psiX);
					uz =  1.0;
					xyzVector[0][0] = Xi - XL;
					xyzVector[1][0] = Yi - YL;
					xyzVector[2][0] = Zi - ZL;
					double **uvw = matrixMultiplyAB(mOpk, xyzVector, 3, 3, 1);

					dUdOmega = (Xi-XL)*dm11dOmega+(Yi-YL)*dm12dOmega+(Zi-ZL)*dm13dOmega;
					dVdOmega = (Xi-XL)*dm21dOmega+(Yi-YL)*dm22dOmega+(Zi-ZL)*dm23dOmega;
					dWdOmega = (Xi-XL)*dm31dOmega+(Yi-YL)*dm32dOmega+(Zi-ZL)*dm33dOmega;

					dUdPhi   = (Xi-XL)*dm11dPhi+(Yi-YL)*dm12dPhi+(Zi-ZL)*dm13dPhi;
					dVdPhi   = (Xi-XL)*dm21dPhi+(Yi-YL)*dm22dPhi+(Zi-ZL)*dm23dPhi;
					dWdPhi   = (Xi-XL)*dm31dPhi+(Yi-YL)*dm32dPhi+(Zi-ZL)*dm33dPhi;

					dUdKappa = (Xi-XL)*dm11dKappa+(Yi-YL)*dm12dKappa+(Zi-ZL)*dm13dKappa;
					dVdKappa = (Xi-XL)*dm21dKappa+(Yi-YL)*dm22dKappa+(Zi-ZL)*dm23dKappa;
					dWdKappa = (Xi-XL)*dm31dKappa+(Yi-YL)*dm32dKappa+(Zi-ZL)*dm33dKappa;

					dUdXL =-mOpk[0][0];
					dVdXL =-mOpk[1][0];
					dWdXL =-mOpk[2][0];

					dUdYL =-mOpk[0][1];
					dVdYL =-mOpk[1][1];
					dWdYL =-mOpk[2][1];

					dUdZL =-mOpk[0][2];
					dVdZL =-mOpk[1][2];
					dWdZL =-mOpk[2][2];

					dUdXi = mOpk[0][0];
					dVdXi = mOpk[1][0];
					dWdXi = mOpk[2][0];

					dUdYi = mOpk[0][1];
					dVdYi = mOpk[1][1];
					dWdYi = mOpk[2][1];

					dUdZi = mOpk[0][2];
					dVdZi = mOpk[1][2];
					dWdZi = mOpk[2][2];

					// Partial derivatives w.r.t. image parameters.
					double uzOverW = uz/uvw[2][0];
					double UOverW = uvw[0][0]/uvw[2][0];
					double VOverW = uvw[1][0]/uvw[2][0];
					B[N_EQN*imagePointIndex][i*N_PAR]    = -uzOverW*(dUdXL-dWdXL*UOverW);
					B[N_EQN*imagePointIndex][i*N_PAR+1]  = -uzOverW*(dUdXL-dWdXL*UOverW)*t;
					B[N_EQN*imagePointIndex][i*N_PAR+2]  = -uzOverW*(dUdXL-dWdXL*UOverW)*t*t;
					B[N_EQN*imagePointIndex][i*N_PAR+3]  = -uzOverW*(dUdYL-dWdYL*UOverW);
					B[N_EQN*imagePointIndex][i*N_PAR+4]  = -uzOverW*(dUdYL-dWdYL*UOverW)*t;
					B[N_EQN*imagePointIndex][i*N_PAR+5]  = -uzOverW*(dUdYL-dWdYL*UOverW)*t*t;
					B[N_EQN*imagePointIndex][i*N_PAR+6]  = -uzOverW*(dUdZL-dWdZL*UOverW);
					B[N_EQN*imagePointIndex][i*N_PAR+7]  = -uzOverW*(dUdZL-dWdZL*UOverW)*t;
					B[N_EQN*imagePointIndex][i*N_PAR+8]  = -uzOverW*(dUdZL-dWdZL*UOverW)*t*t;
					B[N_EQN*imagePointIndex][i*N_PAR+9]  = -uzOverW*(dUdOmega-dWdOmega*UOverW);
					B[N_EQN*imagePointIndex][i*N_PAR+10] = -uzOverW*(dUdOmega-dWdOmega*UOverW)*t;
					B[N_EQN*imagePointIndex][i*N_PAR+11] = -uzOverW*(dUdOmega-dWdOmega*UOverW)*t*t;
					B[N_EQN*imagePointIndex][i*N_PAR+12] = -uzOverW*(dUdPhi-dWdPhi*UOverW);
					B[N_EQN*imagePointIndex][i*N_PAR+13] = -uzOverW*(dUdPhi-dWdPhi*UOverW)*t;
					B[N_EQN*imagePointIndex][i*N_PAR+14] = -uzOverW*(dUdPhi-dWdPhi*UOverW)*t*t;
					B[N_EQN*imagePointIndex][i*N_PAR+15] = -uzOverW*(dUdKappa-dWdKappa*UOverW);
					B[N_EQN*imagePointIndex][i*N_PAR+16] = -uzOverW*(dUdKappa-dWdKappa*UOverW)*t;
					B[N_EQN*imagePointIndex][i*N_PAR+17] = -uzOverW*(dUdKappa-dWdKappa*UOverW)*t*t;

					B[N_EQN*imagePointIndex+1][i*N_PAR]    = -uzOverW*(dVdXL-dWdXL*VOverW);
					B[N_EQN*imagePointIndex+1][i*N_PAR+1]  = -uzOverW*(dVdXL-dWdXL*VOverW)*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+2]  = -uzOverW*(dVdXL-dWdXL*VOverW)*t*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+3]  = -uzOverW*(dVdYL-dWdYL*VOverW);
					B[N_EQN*imagePointIndex+1][i*N_PAR+4]  = -uzOverW*(dVdYL-dWdYL*VOverW)*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+5]  = -uzOverW*(dVdYL-dWdYL*VOverW)*t*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+6]  = -uzOverW*(dVdZL-dWdZL*VOverW);
					B[N_EQN*imagePointIndex+1][i*N_PAR+7]  = -uzOverW*(dVdZL-dWdZL*VOverW)*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+8]  = -uzOverW*(dVdZL-dWdZL*VOverW)*t*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+9]  = -uzOverW*(dVdOmega-dWdOmega*VOverW);
					B[N_EQN*imagePointIndex+1][i*N_PAR+10] = -uzOverW*(dVdOmega-dWdOmega*VOverW)*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+11] = -uzOverW*(dVdOmega-dWdOmega*VOverW)*t*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+12] = -uzOverW*(dVdPhi-dWdPhi*VOverW);
					B[N_EQN*imagePointIndex+1][i*N_PAR+13] = -uzOverW*(dVdPhi-dWdPhi*VOverW)*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+14] = -uzOverW*(dVdPhi-dWdPhi*VOverW)*t*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+15] = -uzOverW*(dVdKappa-dWdKappa*VOverW);
					B[N_EQN*imagePointIndex+1][i*N_PAR+16] = -uzOverW*(dVdKappa-dWdKappa*VOverW)*t;
					B[N_EQN*imagePointIndex+1][i*N_PAR+17] = -uzOverW*(dVdKappa-dWdKappa*VOverW)*t*t;

					// Partial derivatives w.r.t. ground coordinates.
					B[N_EQN*imagePointIndex][N_PAR*mImages+j*N_XYZ]     = -uzOverW*(dUdXi-dWdXi*UOverW);
					B[N_EQN*imagePointIndex][N_PAR*mImages+j*N_XYZ+1]   = -uzOverW*(dUdYi-dWdYi*UOverW);
					B[N_EQN*imagePointIndex][N_PAR*mImages+j*N_XYZ+2]   = -uzOverW*(dUdZi-dWdZi*UOverW);
					B[N_EQN*imagePointIndex+1][N_PAR*mImages+j*N_XYZ]   = -uzOverW*(dVdXi-dWdXi*VOverW);
					B[N_EQN*imagePointIndex+1][N_PAR*mImages+j*N_XYZ+1] = -uzOverW*(dVdYi-dWdYi*VOverW);
					B[N_EQN*imagePointIndex+1][N_PAR*mImages+j*N_XYZ+2] = -uzOverW*(dVdZi-dWdZi*VOverW);

					// F contains values of functions computed from approximations.
					F[N_EQN*imagePointIndex][0]   = -ux + uz*UOverW;
					F[N_EQN*imagePointIndex+1][0] = -uy + uz*VOverW;

					// Wij is an inverse of covariance matrix of each image coordinates.
					W[N_EQN*imagePointIndex][N_EQN*imagePointIndex]     = 1/(groundPoints[j].imagePoints[i].sigmay*groundPoints[j].imagePoints[i].sigmay);
					W[N_EQN*imagePointIndex+1][N_EQN*imagePointIndex+1] = 1/(groundPoints[j].imagePoints[i].sigmax*groundPoints[j].imagePoints[i].sigmax);

					imagePointIndex++;

					// Free allocated memory
					delete [] mOpk;
					delete [] uvw;
				}// end if

			} // finish looping through all images

		} // finish looping through all points

		// Construct Wdot and Fdot
		// Wdot is diagonal and consists of 2 parts:
		// 1. image parameters
		for (int i = 0 ; i < mImages ; i++){
			for (int k = 0 ; k < N_PAR ; k++){
				Wdot[i*N_PAR+k][i*N_PAR+k] = 1/(sigmaImagePar0[i][k]*sigmaImagePar0[i][k]);
			}
		}
		// 2. ground coordinates
		for (int j = 0 ; j < nPoints ; j++){
			Wdot[N_PAR*mImages+j*N_XYZ][N_PAR*mImages+j*N_XYZ]     = 1/(groundPoints[j].sigmaX*groundPoints[j].sigmaX);
			Wdot[N_PAR*mImages+j*N_XYZ+1][N_PAR*mImages+j*N_XYZ+1] = 1/(groundPoints[j].sigmaY*groundPoints[j].sigmaY);
			Wdot[N_PAR*mImages+j*N_XYZ+2][N_PAR*mImages+j*N_XYZ+2] = 1/(groundPoints[j].sigmaZ*groundPoints[j].sigmaZ);
		}
		// Compute N, T, and then Delta
		double **N = matrixMultiplyAtBA(B,W,N_EQN*totalImagePoints,N_PAR*mImages+N_XYZ*nPoints);
		matrixAddToSelf(N,Wdot,N_PAR*mImages+N_XYZ*nPoints,N_PAR*mImages+N_XYZ*nPoints);
		double **T = matrixMultiplyAtBC(B,W,F,N_EQN*totalImagePoints,
					 N_PAR*mImages+N_XYZ*nPoints,N_EQN*totalImagePoints,1);
		double **WdotFdot = matrixMultiplyAB(Wdot,Fdot,N_PAR*mImages+N_XYZ*nPoints,
					      N_PAR*mImages+N_XYZ*nPoints,1);
		matrixSubtractFromSelf(T,WdotFdot,N_PAR*mImages+N_XYZ*nPoints,1);
		double **invN = matrixInverseByLu(N,N_PAR*mImages+N_XYZ*nPoints);
		double **Delta = matrixMultiplyAB(invN,T,N_PAR*mImages+N_XYZ*nPoints,
						 N_PAR*mImages+N_XYZ*nPoints,1);

		// Update parameters for the next iteration
		for (int i=0 ; i<mImages ; i++){
			for (int k=0 ; k<N_PAR ; k++){
				Delta0[i][k]      += Delta[i*N_PAR+k][0];
				Fdot[i*N_PAR+k][0] += Delta[i*N_PAR+k][0];
			}
		}
		
		// Update tie point coordinates
		for (int j=0 ; j<nPoints ; j++){
			if (groundPoints[j].pointType != 'C'){
				groundPoints[j].X += Delta[N_PAR*mImages+N_XYZ*j][0];
				groundPoints[j].Y += Delta[N_PAR*mImages+N_XYZ*j+1][0];
				groundPoints[j].Z += Delta[N_PAR*mImages+N_XYZ*j+2][0];
			}
			Fdot[N_PAR*mImages+N_XYZ*j][0]   += Delta[N_PAR*mImages+N_XYZ*j][0];
			Fdot[N_PAR*mImages+N_XYZ*j+1][0] += Delta[N_PAR*mImages+N_XYZ*j+1][0];
			Fdot[N_PAR*mImages+N_XYZ*j+2][0] += Delta[N_PAR*mImages+N_XYZ*j+2][0];
		}
		// v = f - B*Delta
		double **BDelta = matrixMultiplyAB(B,Delta,N_EQN*totalImagePoints,N_PAR*mImages+N_XYZ*nPoints,1);
		matrixSetZero(v,N_EQN*totalImagePoints,1);
		matrixAddToSelf(v,F,N_EQN*totalImagePoints,1);
		matrixSubtractFromSelf(v,BDelta,N_EQN*totalImagePoints,1);
		matrixSetZero(vdot,N_PAR*mImages+N_XYZ*nPoints,1);
		matrixAddToSelf(vdot,Fdot,N_PAR*mImages+N_XYZ*nPoints,1);
		double **vtWv = matrixMultiplyAtBA(v,W,N_EQN*totalImagePoints,1);
		double **vdottWdotvdot = matrixMultiplyAtBA(vdot,Wdot,N_PAR*mImages+N_XYZ*nPoints,1);

		sigma0hat2 = (vtWv[0][0]+vdottWdotvdot[0][0])/redundancy;

		outputfp.precision(3);
		outputfp.setf(ios::fixed,ios::floatfield); 
		outputfp << "Iteration #" << nIterations << "  sigma0hat2 " << sigma0hat2 << "  vtWv ";
		outputfp << vtWv[0][0] << "  vdottWdotvdot " << vdottWdotvdot[0][0] << endl;

		// Check exit condition
		isOverThreshold = false;
		for (int i=0 ; i<mImages ; i++){
			for (int k=0 ; k<N_PAR ; k++){
				if (abs(Delta[i*N_PAR+k][0]) > 0.0001){
					isOverThreshold = true;
				}
			}
		}
		if ((nIterations < 10) && isOverThreshold){
		  isContinued = true;
		} else {
		  isContinued = false;
		}
		// Free memory
		delete [] N;
		delete [] T;
		delete [] WdotFdot;
		delete [] invN;
		delete [] Delta;
		delete [] BDelta;
		delete [] vtWv;
		delete [] vdottWdotvdot;

	} while (isContinued);

	outputfp << endl << "Adjusted ground point coordinates in geocentric XYZ" << endl;
	for (int j=0 ; j<nPoints ; j++){
		if (groundPoints[j].pointType != 'C'){
			outputfp << "Point # " ;
			outputfp << setw(5) << (groundPoints[j].id).c_str();
			outputfp << ",    X ="  << setw(14) << groundPoints[j].X;
			outputfp << ",  Y =" << setw(14) << groundPoints[j].Y;
			outputfp << ",  Z =" << setw(14) << groundPoints[j].Z << endl;
		}
	}
	
	outputfp << endl << "Adjusted polynomial coefficients " << endl;
	outputfp.precision(6);
	for (int i=0 ; i<mImages ; i++){
		outputfp << "Image " << i << endl ;
		outputfp << "  a0 = " << setw(10) << Delta0[i][0];
		outputfp << ",  a1 = " << setw(10) << Delta0[i][1];
		outputfp << ",  a2 = " << setw(10) << Delta0[i][2] << endl;
		outputfp << "  b0 = " << setw(10) << Delta0[i][3];
		outputfp << ",  b1 = " << setw(10) << Delta0[i][4];
		outputfp << ",  b2 = " << setw(10) << Delta0[i][5] << endl;
		outputfp << "  c0 = " << setw(10) << Delta0[i][6];
		outputfp << ",  c1 = " << setw(10) << Delta0[i][7];
		outputfp << ",  c2 = " << setw(10) << Delta0[i][8] << endl;
		outputfp << "  d0 = " << setw(10) << Delta0[i][9];
		outputfp << ",  d1 = " << setw(10) << Delta0[i][10];
		outputfp << ",  d2 = " << setw(10) << Delta0[i][11] << endl;
		outputfp << "  e0 = " << setw(10) << Delta0[i][12];
		outputfp << ",  e1 = " << setw(10) << Delta0[i][13];
		outputfp << ",  e2 = " << setw(10) << Delta0[i][14] << endl;
		outputfp << "  f0 = " << setw(10) << Delta0[i][15];
		outputfp << ",  f1 = " << setw(10) << Delta0[i][16];
		outputfp << ",  f2 = " << setw(10) << Delta0[i][17] << endl;
	}

	// Free memory
	delete [] mOmega;
	delete [] mPhi;
	delete [] mKappa;
	delete [] xyzVector;
	delete [] B;
	delete [] F;
	delete [] W;
	delete [] Wdot;
	delete [] Fdot;
   return ;
} /* main */
